#include <ESP32CAN.h>
#include <CAN_config.h>
#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>

CAN_device_t CAN_cfg;			  // CAN Config
unsigned long previousMillis = 0; // will store last time a CAN Message was send
const int interval = 20;		  // interval at which send CAN Messages (milliseconds)
const int rx_queue_size = 10;	  // Receive Queue size
//通过CAN通信接收数据
void CAN_Receive_Msg()
{
	CAN_frame_t rx_frame;
	// Receive next CAN frame from queue
	if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE)
	{

		if (rx_frame.FIR.B.FF == CAN_frame_std)
		{
			printf("New standard frame");
		}
		else
		{
			printf("New extended frame");
		}

		if (rx_frame.FIR.B.RTR == CAN_RTR)
		{
			printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID, rx_frame.FIR.B.DLC);
		}
		else
		{
			// char msg[64] = "";
			// for (int i = 0; i < 8; i++)
			// {
			// 	memcpy(msg+i*8, &rx_frame.data.u8[i], 8);
			// }
			float angle1;
			float angle2;
			memcpy(&angle1, &rx_frame.data.u8[0], sizeof(angle1));
			memcpy(&angle2, &rx_frame.data.u8[4], sizeof(angle2));
			printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC);
			printf("%f %f", angle1, angle2);
			// printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID, rx_frame.FIR.B.DLC);
			// for (int i = 0; i < rx_frame.FIR.B.DLC; i++)
			// {
			// 	printf("0x%02X ", rx_frame.data.u8[i]);
			// }
			printf(" RTR %d", rx_frame.FIR.B.RTR);
			printf("\n");
		}
	}
}

void setup()
{
	Serial.begin(115200);
	Serial.println("Basic Demo - ESP32-Arduino-CAN");
	CAN_cfg.speed = CAN_SPEED_1000KBPS;
	CAN_cfg.tx_pin_id = GPIO_NUM_17;
	CAN_cfg.rx_pin_id = GPIO_NUM_16;
	CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
	// Init CAN Module
	ESP32Can.CANInit();
}

void loop()
{
	CAN_Receive_Msg();
}
